/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
 * All rights reserved.</center></h2>
 *
 * This software component is licensed by ST under BSD 3-Clause license,
 * the "License"; You may not use this file except in compliance with the
 * License. You may obtain a copy of the License at:
 *                        opensource.org/licenses/BSD-3-Clause
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include <string.h>
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

float Distance1 = 0;
float Distance1_ = 0;
float Distance2 = 0;
float Distance2_ = 0;
float Distance3 = 0;
float Distance3_ = 0;
float Distance4 = 0;
float Distance4_ = 0;
int Cap_val1 = 0; // 变量1
int Cap_val2 = 0; // 变量2
int Cap_val3 = 0; // 变量3
int Cap_val4 = 0; // 变量4
uint8_t flag1;
uint8_t flag2;
uint8_t flag3;
uint8_t flag4;

// 按键去抖动处理
volatile uint8_t button_pressed = 0;
volatile uint32_t debounce_time2 = 0;
volatile uint32_t debounce_time3 = 0;
volatile uint32_t debounce_time4 = 0;
#define DEBOUNCE_DELAY 200 // 去抖延时，单位：毫秒
#define Dis_Warning 4      // 距离报警阈值 4cm

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/******************* 汽车对应的状态配置 ***************/
/*
                    《状态对应》
                1        对应初始状态
                2        高速行驶阶段
                3        低速行驶阶段
                4        自动泊车阶段
                sxzy     对应上下左右的超声波
 */

int state_index = 0;     // 状态对应的索引
int state_index_old = 0; // 状态对应的索引
/******************* 汽车对应的状态配罿 ***************/

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
int fputc(int ch, FILE *fp)
{
    while (!(USART1->SR & (1 << 7)))
        ;
    USART1->DR = ch;
    return ch;
}
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/******************* 卡尔曼滤波数值配置 ***************/

float P = 1;
float P_; // 对应公式中的p'
float X = 0;
float X_; // X'
float K = 0;
float Q = 0.1; // 噪声
// float R=0.2;  //R如果很大，更相信预测值，那么传感器反应就会迟钝，反之相反
float R = 0.2;

double KLM(double Z)
{
    X_ = X + 0;
    P_ = P + Q;
    K = P_ / (P_ + R);
    X = X_ + K * (Z - X_);
    P = P_ - K * P_;
    return X;
}
/******************* 卡尔曼滤波数值配置 ***************/

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void Start_Trig(void)
{
    // 超声波 4
    HAL_GPIO_WritePin(Trig4_GPIO_Port, Trig4_Pin, GPIO_PIN_SET);
    HAL_Delay(10);
    HAL_GPIO_WritePin(Trig4_GPIO_Port, Trig4_Pin, GPIO_PIN_RESET);

    // 超声波 3
    HAL_GPIO_WritePin(Trig3_GPIO_Port, Trig3_Pin, GPIO_PIN_SET);
    HAL_Delay(10);
    HAL_GPIO_WritePin(Trig3_GPIO_Port, Trig3_Pin, GPIO_PIN_RESET);

    // 超声波 2
    HAL_GPIO_WritePin(Trig2_GPIO_Port, Trig2_Pin, GPIO_PIN_SET);
    HAL_Delay(10);
    HAL_GPIO_WritePin(Trig2_GPIO_Port, Trig2_Pin, GPIO_PIN_RESET);

    // 超声波 1
    HAL_GPIO_WritePin(Trig1_GPIO_Port, Trig1_Pin, GPIO_PIN_SET);
    HAL_Delay(10);
    HAL_GPIO_WritePin(Trig1_GPIO_Port, Trig1_Pin, GPIO_PIN_RESET);
}
/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{
    /* USER CODE BEGIN 1 */

    /* USER CODE END 1 */

    /* MCU Configuration--------------------------------------------------------*/

    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    HAL_Init();

    /* USER CODE BEGIN Init */

    /* USER CODE END Init */

    /* Configure the system clock */
    SystemClock_Config();

    /* USER CODE BEGIN SysInit */
    /* USER CODE END SysInit */

    /* Initialize all configured peripherals */
    MX_GPIO_Init();
    MX_USART1_UART_Init();
    MX_TIM2_Init();
    MX_TIM3_Init();
    MX_TIM4_Init();
    MX_TIM1_Init();
    /* USER CODE BEGIN 2 */
    HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_4); // 使能定时器以及其中断
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_1); // 使能定时器以及其中断
    HAL_TIM_IC_Start_IT(&htim4, TIM_CHANNEL_1); // 使能定时器以及其中断
    HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1); // 使能定时器以及其中断

    // 设置初始状态
    state_index = 1;

    /* USER CODE END 2 */

    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    while (1)
    {
        /* USER CODE END WHILE */

        /* USER CODE BEGIN 3 */

        // 状态的发送   -- 仅当状态改变以后，进行一次发送
        if (state_index_old != state_index)
        {
            printf("%d", state_index);
            state_index_old = state_index;
            HAL_Delay(3000);
        }

        // 异常的发送   -- 仅当出现状态对应的异常时，进行一次发送
        char info[20] = "";

        switch (state_index)
        {
        case 3:
            strcat(info, "3");
            if (Distance3 < Dis_Warning)
                strcat(info, "z");
            if (Distance4 < Dis_Warning)
                strcat(info, "y");
            break;

        case 4:
            strcat(info, "4");
            if (Distance1 < Dis_Warning)
                strcat(info, "s");
            if (Distance2 < Dis_Warning)
                strcat(info, "x");
            if (Distance3 < Dis_Warning)
                strcat(info, "z");
            if (Distance4 < Dis_Warning)
                strcat(info, "y");
            break;
        default:
            break;
        }

        if (strlen(info) >= 2)
        {
            printf("%s", info);
            HAL_Delay(2000);
        }

        Start_Trig(); // 触发信号
        Test3(1);
        // printf("%.2f,%.2f,%.2f,%.2f\r\n", Distance1, Distance2, Distance3, Distance4);
    }
    /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
    RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
    RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
    {
        Error_Handler();
    }
}

/* USER CODE BEGIN 4 */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    // For Echo1
    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4)
    {
        if (flag1 == 0)
        {
            Cap_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_4);
            __HAL_TIM_SET_CAPTUREPOLARITY(&htim2, TIM_CHANNEL_4, TIM_INPUTCHANNELPOLARITY_FALLING);
            flag1 = 1;
        }
        else if (flag1 == 1)
        {
            Distance1 = (HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_4) - Cap_val1) * 0.017;

            if (abs(Distance1 - Distance1_) > 25)
            {
                Distance1 = Distance1_;
            }
            else
            {
                Distance1_ = Distance1;
            }

            __HAL_TIM_SET_CAPTUREPOLARITY(&htim2, TIM_CHANNEL_4, TIM_INPUTCHANNELPOLARITY_RISING);
            flag1 = 0;
            __HAL_TIM_SET_COUNTER(&htim2, 0);
            // Distance1 = KLM(Distance1);
            // Distance1 = Kalman_Filter(&KF, Distance1);
        }
    }
    if (htim->Instance == TIM3)
    {
        // For Echo2
        if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
        {
            if (flag2 == 0)
            {
                Cap_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
                __HAL_TIM_SET_CAPTUREPOLARITY(&htim3, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
                flag2 = 1;
            }
            else if (flag2 == 1)
            {
                Distance2 = (HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1) - Cap_val2) * 0.017;
                if (abs(Distance2 - Distance2_) > 25)
                {
                    Distance2 = Distance2_;
                }
                else
                {
                    Distance2_ = Distance2;
                }
                __HAL_TIM_SET_CAPTUREPOLARITY(&htim3, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
                flag2 = 0;
                __HAL_TIM_SET_COUNTER(&htim3, 0);
                // Distance2 = KLM(Distance2);
            }
        }
    }

    if (htim->Instance == TIM4)
    {
        // For Echo3
        if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
        {
            if (flag3 == 0)
            {
                Cap_val3 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
                __HAL_TIM_SET_CAPTUREPOLARITY(&htim4, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
                flag3 = 1;
            }
            else if (flag3 == 1)
            {
                Distance3 = (HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1) - Cap_val3) * 0.017;
                if (abs(Distance3 - Distance3_) > 25)
                {
                    Distance3 = Distance3_;
                }
                else
                {
                    Distance3_ = Distance3;
                }
                __HAL_TIM_SET_CAPTUREPOLARITY(&htim4, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
                flag3 = 0;
                __HAL_TIM_SET_COUNTER(&htim4, 0);
                // Distance2 = KLM(Distance2);
            }
        }
    }
    if (htim->Instance == TIM1)
    {
        // For Echo4
        if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1)
        {
            if (flag4 == 0)
            {
                Cap_val4 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
                __HAL_TIM_SET_CAPTUREPOLARITY(&htim1, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
                flag4 = 1;
            }
            else if (flag4 == 1)
            {
                Distance4 = (HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1) - Cap_val4) * 0.017;
                if (abs(Distance4 - Distance4_) > 25)
                {
                    Distance4 = Distance4_;
                }
                else
                {
                    Distance4_ = Distance4;
                }
                __HAL_TIM_SET_CAPTUREPOLARITY(&htim1, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
                flag4 = 0;
                __HAL_TIM_SET_COUNTER(&htim1, 0);
                // Distance2 = KLM(Distance2);
            }
        }
    }
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if (GPIO_Pin == key2_Pin)
    {
        if (HAL_GetTick() - debounce_time2 > DEBOUNCE_DELAY)
        {
            debounce_time2 = HAL_GetTick();
            if (HAL_GPIO_ReadPin(key2_GPIO_Port, key2_Pin) == 0)
            {
                state_index = 2;
            }
        }
    }
    else if (GPIO_Pin == key3_Pin)
    {
        if (HAL_GetTick() - debounce_time3 > DEBOUNCE_DELAY)
        {
            debounce_time3 = HAL_GetTick();
            if (HAL_GPIO_ReadPin(key2_GPIO_Port, key2_Pin) == 0)
            {
                state_index = 3;
            }
        }
    }
    else if (GPIO_Pin == key4_Pin)
    {
        if (HAL_GetTick() - debounce_time4 > DEBOUNCE_DELAY)
        {
            debounce_time4 = HAL_GetTick();
            if (HAL_GPIO_ReadPin(key2_GPIO_Port, key2_Pin) == 0)
            {
                state_index = 4;
            }
        }
    }
}

/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
    }
    /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
